//
// Created by bobin on 18-6-25.
//
#include <string>
#include <vector>
#include "dpptam/ua_detrac_loader.h"
#include <chrono>
#include <dpptam/superpixel.h>
#include <dpptam/DenseMapping.h>
#include <dpptam/SemiDenseTracking.h>
#include <dpptam/SemiDenseMapping.h>

using namespace std;

int main(int argc, char **argv) {
    string data_root = "/home/bobin/data/UA_DETRAC/";
    string sequence = "MVI_20011";
//    string sequence = "MVI_39761";
    string train_path = data_root + "Insight-MVT_Annotation_Train/" + sequence;
    string annotation_path = data_root + "annotation-train.txt";
    string config_file = "./src/data-ua.yml";

    std::vector<std::string> image_files;
    std::vector<std::vector<bbox>> box_set;
    load_ua_detrac(annotation_path, train_path, sequence, box_set, image_files);

    cv::FileStorage fs2(config_file, cv::FileStorage::READ);
    DenseMapping dense_mapper(fs2);
    SemiDenseTracking semidense_tracker(fs2);
    SemiDenseMapping semidense_mapper(fs2);
    MapShared Map;
    Imagenes images, images_previous_keyframe;

    int cont_frames;
    double stamps;
    cv::Mat image_frame, image_frame_aux;
    double depth_stamps;


    cont_frames = 0;

    int calculate_superpixels = (int) fs2["calculate_superpixels"];

    ros::Time current_time, stamps_ros;
    semidense_tracker.cont_frames = &cont_frames;
    semidense_tracker.stamps = &stamps;
    semidense_tracker.image_frame = &image_frame_aux;
    semidense_tracker.stamps_ros = &stamps_ros;
    ros::Publisher odom_pub;

    ros::Publisher pub_cloud;
    ros::Publisher pub_poses;
    ros::Publisher vis_pub;
    image_transport::Publisher pub_image;
    ///Launch semidense tracker thread*/
//    boost::thread thread_semidense_tracker(&ThreadSemiDenseTracker, &images, &semidense_mapper, &semidense_tracker,
//                                           &dense_mapper, &Map, &odom_pub, &pub_poses, &vis_pub, &pub_image);
//
//    ///Launch semidense mapper thread
//    boost::thread thread_semidense_mapper(&ThreadSemiDenseMapper, &images, &images_previous_keyframe, &semidense_mapper,
//                                          &semidense_tracker, &dense_mapper, &Map, &pub_cloud);


//    if (calculate_superpixels > 0.5) {
//        ///launch dense mapper thread
//        boost::thread thread_dense_mapper(&ThreadDenseMapper, &dense_mapper, &pub_cloud);
//    }

    cout << "*** DPPTAM is working     *** " << endl << endl;
    cout << "*** Launch the example sequences or use your own sequence ***" << endl;

    for (unsigned j = 1; j < image_files.size(); ++j) {
        cv::Mat whole_image = cv::imread(image_files[j], cv::IMREAD_UNCHANGED);
        if (whole_image.empty()) {
            continue;
        }
        cv::Mat whole_gray;
        cv::cvtColor(whole_image, whole_gray, cv::COLOR_RGB2GRAY);
        std::vector<bbox> &boxes = box_set[j];
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
        stamps = j / 10;
        stamps_ros = ros::Time(stamps);
        current_time = stamps_ros;

        semidense_tracker.add_frame(whole_gray);
        auto & box = boxes[1];
        int box_right = box.left + box.width;
        int box_bottom = box.top + box.height;
        if (box_right  == whole_gray.cols + 1) {
            box.width -= 1;
        }

        if (box_bottom  == whole_gray.rows + 1) {
            box.height -= 1;
            box_bottom = box.top + box.height;
        }
        cv::Rect rect(box.left, box.top, box.width, box.height);
        cv::Mat src = whole_gray(rect);
        image_frame_aux = src;
        cont_frames++;
        semidense_tracker.cont_frames = &cont_frames;
        semidense_tracker.stamps = &stamps;
        semidense_tracker.image_frame = &image_frame_aux;
        semidense_tracker.stamps_ros = &stamps_ros;
        std::cout << "frame :" << j << std::endl;

        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
        double timeCost = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
        std::cout << "time dpptam: " << timeCost << std::endl;

        sleep(0.1);
    }
//    thread_semidense_mapper.join();
//    thread_semidense_tracker.join();
}